//*************************************************************************************
/** \file task_motor.cpp
 *    This file contains a task class for causing a motor to move a lever back and 
 *    forth. It's for a simple ME405 demonstration. When run, the task reads the 
 *    position of a lever with a potentiometer and uses the reading to move the
 *    lever in a back and forth motion -- unless someone has sent a command to this
 *    task telling it to stop. 
 *
 *  Revisions:
 *    \li 02-06-2008 JRR Original file
 *    \li 01-18-2009 JRR Changed into more direct dual motor test program
 *    \li 01-19-2011 JRR Updated calls to newer version of stl_task constructor
 *
 *  License:
 *    This file released under the Lesser GNU Public License, version 2. This program
 *    is intended for educational use only, but it is not limited thereto. 
 */
//*************************************************************************************


#include <stdlib.h>
#include <avr/io.h>
#include "rs232int.h"
#include "stl_timer.h"
#include "stl_task.h"
#include "global_debug.h"
#include "avr_adc.h"
#include "motor405.h"
#include "task_motor.h"


//-------------------------------------------------------------------------------------
/** This constructor creates a motor task object. The motor object needs pointers to
 *  an A/D converter and two motor controllers in order to do its thing, and a serial
 *  port pointer is handy for debugging. 
 *  @param a_timer A reference to the real-time measuring timer for the task
 *  @param t_stamp A timestamp which contains the time between runs of this task
 *  @param p_mot_1 A pointer to the first motor controller object
 *  @param p_mot_2 A pointer to the second motor controller object
 *  @param p_a2d   A pointer to an analog to digital converter object
 *  @param p_ser   A pointer to a serial port for sending messages (default NULL)
 *  WARNING:  This task uses an older version of parent class stl_task, and its 
 *            constructor parameters are out of date. Use it as an example, but
 *            do not attempt to just copy the parameters. 
 */

task_motor::task_motor (task_timer& a_timer, time_stamp& t_stamp, motor405* p_mot_1, 
						motor405* p_mot_2, avr_adc* p_a2d)
	: stl_task (a_timer, t_stamp)
{
	p_motor_1 = p_mot_1;					// Save pointers to other objects
	p_motor_2 = p_mot_2;
	p_adc = p_a2d;

	// Set the motors' power (or brake) duty cycle
	p_motor_1->set_duty_cycle (0);
	p_motor_2->set_duty_cycle (0);
	
	// Set the direction
	direction = true;

	// Say hello
	GLOB_DEBUG ("Motor task constructor" << endl);
}


//-------------------------------------------------------------------------------------
/** This is the function which runs when it is called by the task scheduler. It causes
 *  the motor to move at a speed determined by the A/D converter, which is expected to
 *  be hooked up to a couple of potentiometers. 
 *  @param state The state of the task when this run method begins running
 *  @return The state to which the task will transition, or STL_NO_TRANSITION if no
 *	  transition is called for at this time
 */

char task_motor::run (char state)
{
	uint32_t duty_cycle;					// Used to compute motor PWM duty cycle
	const char TM_OVERSAMPLE = 10;			// How many times to oversample the A/D

	switch (state)
	{
		// In State 0, the motor should stop
		case (0):
			p_motor_1->brake ();
			p_motor_2->brake ();

			// If somebody pressed the go button, transition to the go state
			if (going)
			{
				GLOB_DEBUG ("Motor going" << endl);
				return (1);
			}
			else
				return (STL_NO_TRANSITION);
			break;

		// State 1 is about the motor going somewhere
		case (1):
			// Set the direction
			if (direction)
			{
				p_motor_1->clockwise ();
				p_motor_2->clockwise ();
			}
			else
			{
				p_motor_1->counterclockwise ();
				p_motor_2->counterclockwise ();
			}

			// Set the motor speed according to the throttle potentiometers
			duty_cycle = 0L;
			for (char index = 0; index < TM_OVERSAMPLE; index++)
				duty_cycle += p_adc->read_once (0);
			duty_cycle /= TM_OVERSAMPLE;
			duty_cycle *= 255;
			duty_cycle /= 1023;
			p_motor_1->set_duty_cycle ((uint8_t)duty_cycle);

			duty_cycle = 0L;
			for (char index = 0; index < TM_OVERSAMPLE; index++)
				duty_cycle += p_adc->read_once (1);
			duty_cycle /= TM_OVERSAMPLE;
			duty_cycle *= 255;
			duty_cycle /= 1023;
			p_motor_2->set_duty_cycle ((uint8_t)duty_cycle);

			// If somebody pressed the stop button, transition to the stopped state
			if (!going)
		{
				GLOB_DEBUG ("Motor stopping" << endl);
				return (0);
		}
			else
				return (STL_NO_TRANSITION);
			break;

		// If the state isn't a known state, call Houston; we have a problem
		default:
			GLOB_DEBUG ("WARNING: Motor control task in state " << state << endl);
			return (0);
	};

	// If we get here, no transition is called for
	return (STL_NO_TRANSITION);
}


//-------------------------------------------------------------------------------------
/** This method is called to tell the motor that it should go. 
 */

void task_motor::go (void)
{
	going = true;
}
		

//-------------------------------------------------------------------------------------
/** This method is called to tell the motor to stop moving. 
 */

void task_motor::stop (void)
{
	going = false;
}
